#include "os_queue.h"
#include "os_mem.h"
#include "os_list.h"
#include "os_list.h"
#include "os_dbg.h"
#include "os_task.h"
#include "os_sched.h"
#include "os_lib.h"

#if OS_QUEUE_ENABLE

#if !OS_QUEUE_DBG_PRINT_ENABLE
#undef os_dbg
#define os_dbg(fmt, ...) do {}while (0)
#endif

/**
 * 消息等待的结构
 */
typedef struct _queue_wait_t {
    void * msg;
    int opt;
}queue_wait_t;


/**
 * 初始化消息队列
 * @param queue 待初始化的消息队列
 * @param size 队列大小
 * @param msg_buf 消息存储的位置
 * @param flags 相关标志
*/
static os_err_t queue_init (os_queue_t * queue, void * pool, int msg_size, int msg_cnt, int flags) {
    // 初始化等待结构
    os_err_t err = os_event_init(&queue->read_event, OS_EVENT_TYPE_QUEUE, flags);
    if (err < 0) {
        os_dbg("create queue in_event failed: %s.", "init failed.");
        return err;
    }
    // 初始化等待结构
    err = os_event_init(&queue->write_event, OS_EVENT_TYPE_QUEUE, flags);
    if (err < 0) {
        os_event_uninit(&queue->read_event);
        os_dbg("create queue out_event failed: %s.", "init failed.");
        return err;
    }

    queue->read = queue->write = 0;
    queue->msg_cnt = 0;
    queue->msg_size = msg_size;
    queue->msg_max = msg_cnt;
    queue->s_pool = pool;
    queue->total_size = msg_size * msg_cnt;
    return OS_ERR_OK;
}

/**
 * 初始化消息队列
 * @param queue 待初始化的消息队列
 * @param size 队列大小
 * @param msg_buf 消息存储的位置
 * @return 初始化结果
*/
os_err_t os_queue_init (os_queue_t * queue, void * buf, uint16_t msg_size, uint16_t msg_cnt) {
    os_param_failed(queue == OS_NULL, OS_ERR_PARAM);
    os_param_failed(buf == OS_NULL, OS_ERR_PARAM);
    os_param_failed(msg_size <= 0, OS_ERR_PARAM);
    os_param_failed(msg_cnt <= 0, OS_ERR_PARAM);
    return queue_init(queue, buf, msg_size, msg_cnt, 0);
}

/**
 * 取消消息队列的初始化
 * @param queue 消息队列
*/
os_err_t os_queue_uninit (os_queue_t * queue) {       
    os_param_failed(queue == OS_NULL, OS_ERR_PARAM);

    // 唤醒所有任务后，尝试进行调试
    os_int_status_t status = os_sched_int_lock();
    int cnt = os_event_wait_cnt(&queue->read_event) + os_event_wait_cnt(&queue->write_event);
    os_event_uninit(&queue->read_event);
    os_event_uninit(&queue->write_event);
    os_sched_int_restore(status);

    // 有任务唤醒时，进行调度，以便运行更高优先级的任务
    if (cnt > 0) {
        os_sched_run();
    } 

    return OS_ERR_OK;
}

#if OS_MEM_ENABLE

/**
 * 分配一个消息队列
 * @param size 消息队列的大小
 * @return 创建的消息队列
 */
os_queue_t * os_queue_create (uint16_t msg_size, uint16_t msg_cnt) {
    os_param_failed(msg_size <= 0, OS_NULL);
    os_param_failed(msg_cnt <= 0, OS_NULL);

    // 分配内存
    os_queue_t * queue = os_mem_malloc(sizeof(os_queue_t));
    if (queue == OS_NULL) {
        os_dbg("error: alloc queue failed");
        return OS_NULL;
    }

    // 分配存储空间，注意size对齐到int大小的边界
    void * pool = os_mem_malloc(msg_size * msg_cnt);
    if (pool == OS_NULL) {
        os_dbg("error: alloc msg pool failed");
        os_mem_free(queue);
        return OS_NULL;
    }

    // 初始化消息队列结构
    os_err_t err = queue_init(queue, pool, msg_size, msg_cnt, OS_OBJECT_HEAP);
    if (err < 0) {
        os_dbg("error: init queue failed.");
        os_mem_free(pool);
        os_mem_free(queue);
        return OS_NULL;
    }

    return queue;
}

/**
 * 释放一个消息队列
 * @param 需要释放的消息队列
 */
os_err_t os_queue_free (os_queue_t * queue) {
    os_param_failed(queue == OS_NULL, OS_ERR_PARAM);

    os_err_t err = os_queue_uninit(queue);
    if (err < 0) {
        os_dbg("queue uninit error");
        return err;
    }

    if (queue->read_event.flags & OS_OBJECT_HEAP) {
        os_mem_free(queue->s_pool);
        os_mem_free(queue);
    }

    return OS_ERR_OK;
}
#endif

#if OS_QUEUE_INFO_ENABLE
/**
 * 获取消息数量
 * @param queue 消息队列
 * @return 消息数量
*/
uint16_t os_queue_msg_cnt (os_queue_t * queue) {
    os_param_failed(queue == OS_NULL, -1);

    os_int_status_t status = os_sched_int_lock();
    uint16_t cnt = queue->msg_cnt;
    os_sched_int_restore(status);
    return cnt;
}

/**
 * 获取消息队列的大小
 * @param queue 存储块结构
 * @return 队列大小
*/
uint16_t os_queue_free_cnt (os_queue_t * queue) {
    os_param_failed(queue == OS_NULL, -1);

    os_int_status_t status = os_sched_int_lock();
    uint16_t cnt = queue->msg_max - queue->msg_cnt;
    os_sched_int_restore(status);
    return cnt;
}

/**
 * 获取等待消息的任务数量
 * @param qeueue 消息队列
 * @return 等待消息的任务数量
 */
uint16_t os_queue_read_task_cnt(os_queue_t * queue) {
    os_param_failed(queue == OS_NULL, -1);

    os_int_status_t status = os_sched_int_lock();
    uint16_t cnt = os_event_wait_cnt(&queue->read_event);
    os_sched_int_restore(status);
    return cnt;
}

/**
 * 获取等待释放消息的任务数量
 * @param qeueue 消息队列
 * @return 等待消息的任务数量
 */
uint16_t os_queue_write_task_cnt(os_queue_t * queue) {
    os_param_failed(queue == OS_NULL, -1);

    os_int_status_t status = os_sched_int_lock();
    uint16_t cnt = os_event_wait_cnt(&queue->write_event);
    os_sched_int_restore(status);
    return cnt;
}
#endif

/**
 * 清空消息队列
 * @param queue 待清空的消息队列
 */
os_err_t os_queue_clear (os_queue_t * queue) {
    os_param_failed(queue == OS_NULL, -1);

    os_int_status_t status = os_sched_int_lock();
    
    // 释放所有等待的任务
    os_event_clear(&queue->read_event, OS_NULL, OS_ERR_CLEAR);
    os_event_clear(&queue->write_event, OS_NULL, OS_ERR_CLEAR);

    // 重设指针
    queue->msg_cnt = 0;
    queue->read = queue->write = 0;
    
    os_sched_int_restore(status);
    return OS_ERR_OK;
}

/**
 * 从消息队列中读取一个消息到指定的位置
 */
static void queue_read_msg (os_queue_t * queue, int opt, void * msg) {
    // 从消息队列中复制出消息
    os_memcpy(msg, queue->s_pool + queue->read, queue->msg_size);

    // 调整消息指针
    queue->read += queue->msg_size;
    if (queue->read >= queue->total_size) {
        queue->read = 0;
    }
    queue->msg_cnt--;
}

/**
 * 向消息队列中写入一个消息
 * 消息可以往队列头部也可以往队列尾部写消息
 */
static void queue_write_in (os_queue_t * queue, void * msg, int opt) {
    if (opt & OS_QUEUE_RELEASE_FRONT) {
#if OS_QUEUE_RELEASE_FRONT_ENABLE
        queue->read -= queue->msg_size;
        if (queue->read < 0) {
            queue->read = queue->total_size - queue->msg_size;
        }

        os_memcpy((uint8_t *)queue->s_pool + queue->read, msg, queue->msg_size);
#endif
    } else {
        os_memcpy((uint8_t *)queue->s_pool + queue->write, msg, queue->msg_size);

        queue->write += queue->msg_size;
        if (queue->write >= queue->total_size) {
            queue->write = 0;
        }
    }
    queue->msg_cnt++;
}

/**
 * 等待消息队列中的消息
 * @param queue 等待的消息队列
 * @param ms 等待的毫秒数
 * @param msg 取得的消息
*/
os_err_t os_queue_read (os_queue_t * queue, int ms, int opt, void * msg) {
    os_param_failed(queue == OS_NULL, OS_ERR_PARAM);
    os_param_failed(msg == OS_NULL, OS_ERR_PARAM);

    os_int_status_t status = os_sched_int_lock();
    if (queue->msg_cnt > 0) {
        // 如果有消息，取出一个消息
        queue_read_msg(queue, opt, msg);

        // 唤醒等待写消息的线程
        os_task_t * task = os_event_wakeup(&queue->write_event);
        if (task) {
            task->event_wait.err = OS_ERR_OK;

            // 从发送方方中提取中消息，然后写入到消息队列中，总消息数保持
            queue_wait_t * q_wait = (queue_wait_t *)task->event_wait.reason;
            queue_write_in(queue, q_wait->msg, q_wait->opt);
        }

        // 写入完毕后退出
        os_sched_int_restore(status);
        return OS_ERR_OK;
    } else if (ms < 0) {
        // 无消息，且不等待，直接退出
        os_dbg("msg count == 0 and no wait\n");
        os_sched_int_restore(status);
        return OS_ERR_NONE;
    } else {
        // 没有，等待消息。如果有消息，会由发送方将消息写入到msg中
        queue_wait_t q_wait = {.opt = opt, .msg = msg}; 
        os_event_wait(&queue->read_event, &q_wait, ms);
        os_sched_run();
        os_sched_int_restore(status);

        // 从上面退出之后，是否取得消息，由err判断
        os_task_t * self = os_task_self();
        return self->event_wait.err;
    }
}

/**
 * 释放消息
 * @param queue 消息队列
 * @param msg 待释放的消息
 * @return 释放的结果
 */
os_err_t os_queue_write (os_queue_t * queue, int ms, int opt, void * msg) {
    os_param_failed(queue == OS_NULL, OS_ERR_PARAM);
    os_param_failed(msg == OS_NULL, OS_ERR_PARAM);
    os_assert(queue->write_event.type == OS_EVENT_TYPE_QUEUE);

    os_int_status_t status = os_sched_int_lock();
    os_task_t * task = os_event_wakeup(&queue->read_event);
    if (task) {
        // 有任务等消息，将消息写入任务读取的缓冲区
        os_wait_t * wait = &task->event_wait;
        wait->err = OS_ERR_OK; 

        queue_wait_t * q_wait = (queue_wait_t *)wait->reason;
        os_memcpy(q_wait->msg, msg, queue->msg_size);

        os_sched_int_restore(status);
        return OS_ERR_OK;
    } else if (queue->msg_max > queue->msg_cnt) {
        // 有空闲空间，写入队列中
        queue_write_in(queue,msg, opt);

        os_sched_int_restore(status);
        return OS_ERR_OK;
    } if (ms < 0) {
        // 无空闲，不等待
        os_sched_int_restore(status);

        os_dbg("queue full and no wait\n");
        return OS_ERR_FULL;
    } else {
        // 没有空闲空间，任务需要等, 该消息会在队列中的其它消息被读取之后立即写入
        queue_wait_t q_wait = {.opt = opt, .msg = msg}; 
        os_event_wait(&queue->write_event, &q_wait, ms);
        os_sched_run();
        os_sched_int_restore(status);

        // 判断是否发送成功
        os_task_t * self = os_task_self();
        return self->event_wait.err;
    }
}

#endif // OS_QUEUE_ENABLE
